/**
 * @file	cat_controller.c
 * @author	Mitch, David
 * @brief	Cat controller. Creates 3 tasks: Sending, Receiving, and Control.
 * 			Sending and Receiving tasks are system tasks, and control task is BRR.
 * 			Control task is the brain of the cat. It polls sensors to detect the mouse
 * 			will execute chasing or searching commands accordingly.
 */

#include "header/common.h"
#include "header/os.h"
#include "roomba/roomba.h"
#include "roomba/roomba_sci.h"
#include "radio/radio.h"
#include "uart/uart.h"
#include "header/sensor.h"
#include "header/cat_controller.h"
#include <stdlib.h>

#ifdef CAT_CONTROLLER

/** RTOS requires these PPP arrays definitions */
const unsigned char PPP[] = {};
const unsigned int PT = sizeof(PPP)/2;

/** Sensor direction variable */
IR_Direction_t direction = IR_NONE;

/** Event variable that is signalled or waited upon for sending through radio */
EVENT *sendEvent;

/** Event variable that is signalled or waited upon for receiving through radio */
EVENT  *receiveEvent;

/** flag to restrict send event */
int send_flag = 0;

/** flag to test for game over */
int game_over_flag = 0;

/** flag to test if cat is winner */
int cat_wins = 0;

/** Radio variables needed */
uint8_t mouse_addr[5] = { 0x11, 0x11, 0x11, 0x11, 0x11 }; //!< mouse address
uint8_t cat_addr[5] = { 0x12, 0x12, 0x12, 0x12, 0x12 }; //!< cat address
radiopacket_t command_packet; //!< packet to be sent containing movement command
radiopacket_t status_packet; //!< status packet to be received
volatile uint8_t rxflag = 0; //!< Radio Receive Flag

/**
 * @brief 	LED initializaton function. Initialize ports PD4 to PD7 to use LEDs.
 */
void init_LED(void) {
	DDRD = (1<<PORTD4) | (1<<PORTD5) | (1<<PORTD6) | (1<<PORTD7);
}

/**
 * @brief 	Initialize Radio.
 */
void init_radio(void) {
	Radio_Init();

	Radio_Configure_Rx(RADIO_PIPE_0, cat_addr, ENABLE);

	Radio_Configure(RADIO_2MBPS, RADIO_HIGHEST_POWER);

	Radio_Set_Tx_Addr(mouse_addr);

	set_roomba_command(&command_packet, DRIVE);
}

/**
 * @brief 	Roomba Driving function. Parameters are velocity and radius.
 */
void roomba_Drive( int16_t velocity, int16_t radius )
{
	uart_putchar(DRIVE);
	uart_putchar(HIGH_BYTE(velocity));
	uart_putchar(LOW_BYTE(velocity));
	uart_putchar(HIGH_BYTE(radius));
	uart_putchar(LOW_BYTE(radius));
}

/**
 * @brief 	Get bumper status. Return 0 if not activated, otherwise 1.
 */
int get_bumper_status(void) {
	/** set roomba to sensor data mode */
	uart_putchar(SENSORS);

	/** ask roomba for external sensor data */
	uart_putchar(1);

	/** retrieve first byte corresponding to bumper status */
	while (uart_bytes_received() != 10);

	/** store bumper status information in local variable */
	uint8_t bumper_status = uart_get_byte(0);

	/** reset received when done */
	uart_reset_receive();

	/** if bumper is not activated, return 0 */
	if (bumper_status == 0) {
		return 0;
	} else { /** if bumper is activate, return 1 */
		return 1;
	}
}

/**
 * @brief 	Search for n ticks where n x 5 milliseconds = number of milliseconds searching
 */
void search_n_ticks(int n) {
	unsigned int start = Now();
	int bumper_status = 0;

	for (;;) {
		/** if reaching search tick limit, break */
		if ((Now() - start) >= n) {
			Task_Next();
			break;
		} else { /** check whether bumper is activated */
			bumper_status = get_bumper_status();

			/** if bumper is not activated, turn on red LED on PD4 */
			if (bumper_status == 0) {
			} else { /** if bumper is activated, stop roomba and signal sendEvent */
				roomba_Drive(0,0);
				send_flag = 1;
				Signal_And_Next(sendEvent);
			}
		}
	}
}

/*
 * @brief 	Prepare roomba command packet.
 *
 * @param 	packet a packet to be sent to Roomba
 * @param 	command a command to be sent to Roomba
 * */
void set_roomba_command(radiopacket_t *packet, uint8_t command) {
	//copy sender address to packet
	memcpy(command_packet.payload.command.sender_address, cat_addr, 5);
	packet->type = COMMAND;
	packet->payload.command.command = command;
}

/**
 * @brief 	Radio Send task. Sends a packet asking the mouse for bumper and sensor status.
 */
void radio_tx(void) {
	for (;;) {
		if (send_flag) {
			send_flag = 0;
			Radio_Transmit(&command_packet, RADIO_WAIT_FOR_TX);
		}
		Event_Wait(sendEvent);
	}
}

/**
 * @brief 	Radio Receive task. Receives reply from mouse of bumper and sensor status.
 * 		  	Game ends if bumper and sensor status are activated, otherwise cat continues chasing mouse.
 */
void radio_rx(void) {
	int turn_flag = 0;
	for (;;) {
		/** upon receiving status packet from roomba */
		if (rxflag) {
			_delay_ms(20);

			if (Radio_Receive(&status_packet) != RADIO_RX_MORE_PACKETS) {
				rxflag = 0;
			}
			if (status_packet.payload.command.command == POWER) {
				game_over_flag = 1;
			}
			else {
				/** Cat has win the game */
				if (status_packet.payload.sensors.sensors.bumps_wheeldrops != 0 && status_packet.payload.sensors.sensors.wall != 0) {
					game_over_flag = 1;
					cat_wins = 1;
				}
				else {
					roomba_Drive(0xFFCE, 0x8000);
					_delay_ms(1000);
					if (turn_flag == 0) {
						roomba_Drive(50, 0x0001);
						_delay_ms(1000);
						turn_flag = 1;
					}
					else {
						roomba_Drive(50, 0xFFFF);
						_delay_ms(1000);
						turn_flag = 1;
					}
					roomba_Drive(0,0);
					_delay_ms(1000);
				}
			}
		}
		Event_Wait(receiveEvent);
	}
}

/**
 * @brief 	Control task. This task polls sensor to detect mouse. If it detects the mouse
 * 			it will execute a chasing command. Otherwise, it executes a searching command.
 * 			A chasing command is a roomba movement in the direction of the mouse, executed
 * 			by sending an sci command to the roomba. A searching command is a random
 * 			roomba command executed by using the rand() function.
 */
void control_task(void) {
	for (;;) {
		if (game_over_flag) {
			if (cat_wins) {
				PORTD ^=_BV(PD6); //green
			}
			roomba_Drive(0,0);
			Task_Terminate();
		}
		direction = get_current_direction();

		if (direction == IR_WEST) {
			roomba_Drive(250, 0x0001);
			search_n_ticks(200);
		}
		else if (direction == IR_EAST) {
			roomba_Drive(250, 0xFFFF);
			search_n_ticks(200);
		}
		else if (direction == IR_SOUTH) {
			roomba_Drive(250, 0x0001);
			search_n_ticks(400);
			roomba_Drive(250, 0x8000);
			search_n_ticks(200);
		}
		else if (direction == IR_NORTH) {
			roomba_Drive(250, 0x8000);
			search_n_ticks(200);
		} else {
			/** Randomly select a movement command to search for the mouse */
			switch (rand() % 4) {
				case 0:
					roomba_Drive(100,0x0001);
					break;
				case 1:
					roomba_Drive(100,0xFFFF);
					break;
				case 2:
					roomba_Drive(0xFF9C, 0x8000);
					break;
				case 3:
					roomba_Drive(100,0x8000);
					break;
				default:
					roomba_Drive(0,0);
					break;
			}
			search_n_ticks(200);
		}
	}
}

/**
 * @brief	Cat Main function. Initialize variables, and instantiate tasks.
 */
int main(void) {

	/** Set clock speed to 8MHz */
	clock8MHz();

	/** disable interrupts */
	cli();

	/** initialize LEDs */
	init_LED();

	/** initialize Roomba */
	Roomba_Init();

	/** initialize Radio */
	init_radio();

	/** enable interrupts */
	sei();

	/** initialize events */
	sendEvent = Event_Init();
	receiveEvent = Event_Init();

	/** Create a system task for sending packet to mouse */
	Task_Create(radio_tx,1,SYSTEM,1);

	/** Create a system task for receiving packet from mouse */
	Task_Create(radio_rx,2,SYSTEM,2);

	/** Create a BRR task for searching */
	Task_Create(control_task, 0, BRR, 0);

	return 0;
}

/**
 * @brief	Interrupt function for radio rx.
 */
void radio_rxhandler(uint8_t pipenumber)
{
	rxflag = 1;
	Event_Signal(receiveEvent);
}

#endif
